import numpy as np
import control as ct
from math import cos,sin,tan
from Plantdata import *
g=9.80665
#角速度内环控制器，利用NonelinearIOSystem实现NDI控制
class NDI_pqr:
    #input[p q r]desire
    #output[dle dla dlr]
    def __init__(self,A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la):
        #self.x_dot0=np.array([0.0,0]).T
        self.A_long=A_long
        self.A_la=A_la
        self.B_long=B_long
        self.B_la=B_la
        self.C_long=C_long
        self.C_la=C_la
        self.D_long=D_long
        self.D_la=D_la
        #self.G=np.eye(3)
        self.NDI_pre=ct.NonlinearIOSystem(
            self.NDI_update,
            self.NDI_output,
            name="NDI_pqr",
            input=("desire_p","desire_q","desire_r","v","alpha","q","theta","beta","p","r","phi"),
            output=("de","da","dr"),
            states=("de","da","dr")
        )
    def NDI_update(self,t,x,u,params):
        return np.zeros((3,))
    ##所以这里的状态量全为0，因为只用了输出函数
    def calf(self,V,alpha,q,theta,beta,p,r,phi):
        f=np.array([
            sum(self.A_la[1,:]*[beta,p,r,phi]),
            sum(self.A_long[2,:]*[V,alpha,q,theta]),
            sum(self.A_la[2,:]*[beta,p,r,phi])
        ])
        return f.reshape((3,1))
    def NDI_output(self,t,x,u,params):
        G=np.array([
            [1,0,0],
            [0,1,0],
            [0,0,1]
        ])
        G=params.get('G_pqr',np.eye(3))
        G_inv=np.linalg.inv(G)
        v=u[0:3].reshape((3,1))

        [V,alpha,q,theta,beta,p,r,phi]=u[3:11]
        f=self.calf(V,alpha,q,theta,beta,p,r,phi)
        du=(G_inv@(np.eye(3)@v-f)).T
        #self.x_dot0=x
        #print("NDI_pqr",du)
        return  du.reshape((3,))
    def getsys(self):
        return self.NDI_pre

#角度外环控制器
class NDI_alphabetaphi:
    #[alpha beta phi]
    def __init__(self,A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la):
        self.A_long=A_long
        self.A_la=A_la
        self.B_long=B_long
        self.B_la=B_la
        self.C_long=C_long
        self.C_la=C_la
        self.D_long=D_long
        self.D_la=D_la
        self.da=0
        self.de=0
        self.dr=0

        self.NDI=ct.NonlinearIOSystem(
            self.NDI_update,
            self.NDI_output,
            name="NDI",
            input=("desire_alpha","desire_beta","desire_phi","v","alpha","q","theta","beta","p","r","phi","de","da","dr"),
            output=("desire_p","desire_q","desire_r"),
            states=("alpha","beta","phi")
        )
    def NDI_update(self,t,x,u,params):
        [V,alpha,q,theta,beta,p,r,phi,de,da,dr]=u[3:14]
        self.da=da
        self.de=de
        self.dr=dr
        return np.zeros((3,))
    def calf(self,V,alpha,q,theta,beta,p,r,phi,de,da,dr,T,m,L_v,L_a,Y_b,v0,alpha0):
        #alpha=alpha+alpha0
        #V=V+v0
        #print(alpha,V)
        f=[
            sum(self.A_long[1,[0,1,3]]*[V,alpha,theta]),#+sum(self.B_long[2,[0]]*[self.de]),
            sum(self.A_la[0,[0,3]]*[beta,phi]),#+sum(self.B_la[0,[0,1]]*[self.da,self.dr]),
            sum(self.A_la[3,[0,3]]*[beta,phi]),#+sum(self.B_la[3,[0,1]]*[self.da,self.dr]),
            #(-T*cos(alpha)-(L_v*V+L_a*alpha)-m*g*(sin(alpha)*sin(theta)+cos(alpha)*cos(phi)*cos(theta)))/(m*V*cos(beta)),
            #(-T*cos(alpha)*cos(beta)+(Y_b*beta)+m*g*(cos(alpha)*sin(beta)*sin(theta)+cos(beta)*sin(phi)*cos(theta)-sin(alpha)*sin(beta)*cos(phi)*cos(theta)))/(m*V),
           # 0,
        ]
        f=np.array(f)
        #print(f)
        return f.reshape((3,1))
    def NDI_output(self,t,x,u,params):
        G=params.get('G_alphabetaphi',np.eye(3))
        G_inv=np.linalg.inv(G)
        v=u[0:3].reshape((3,1))
        #V=u[-1]
        [V,alpha,q,theta,beta,p,r,phi,de,da,dr]=u[3:14]
        T=params.get('T',10000)
        m=params.get('m',16000)
        L_v=params.get('L_v',60)
        L_a=params.get('L_a',5)
        Y_b=params.get('Y_b',-90)
        v0=params.get('v0',91.2)
        alpha0=params.get('alpha0',17.56)
        alpha0=np.deg2rad(alpha0)
        f=self.calf(V,alpha,q,theta,beta,p,r,phi,de,da,dr,T,m,L_v,L_a,Y_b,v0,alpha0)
        #print(v-f)
        du=(G_inv@(v-f)).T
        #self.x_dot0=x
        #print("u_NDI",du)
        #print("NDI",du)
        return  du.reshape((3,))
    
    
    def getsys(self):
        return self.NDI
class Plane:
    def __init__(self,A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la):
        self.A_long=A_long
        self.A_la=A_la
        self.B_long=B_long
        self.B_la=B_la
        self.C_long=C_long
        self.C_la=C_la
        self.D_long=D_long
        self.D_la=D_la

        self.p_dot=0
        self.q_dot=0
        self.r_dot=0
        self.plane_sys=ct.NonlinearIOSystem(
            self.Plane_update,
            self.Plane_output,
            name="Plane",
            input=("de","da","dr"),
            output=("v","alpha","q","theta","beta","p","r","phi","p_dot","q_dot","r_dot","de","da","dr"),
            states=("v","alpha","q","theta","beta","p","r","phi")
        )
    def Plane_update(self,t,x,u,params):
        #print('update')
        x_long=x[0:4].reshape((4,1))
        x_la=x[4:8].reshape((4,1))
        u_long=u[0]
        u_la=u[1:3].reshape((2,1))

        #d_ear=np.zeros((3,1)).T

        self.p_dot=x_la[1]
        self.q_dot=x_long[2]
        self.r_dot=x_la[2]

        #print("u_long",u_long)

        dx_long=(self.A_long@(x_long)+self.B_long*(u_long)).T
        dx_la=(self.A_la@(x_la)+self.B_la@(u_la)).T

        #print(dx_long)
        return np.hstack((dx_long,dx_la))
    def Plane_output(self,t,x,u,params):
        #print(u.shape)
        x_long=x[0:4].reshape((4,1))
        x_la=x[4:8].reshape((4,1))
        u_long=u[0]
        u_la=u[1:3].reshape((2,1))
        r_x_long=(self.C_long@(x_long)+self.D_long*(u_long)).T.reshape((4,))
        r_x_la=(self.C_la@(x_la)+self.D_la@(u_la)).T.reshape((4,))
        pqr_dot=np.array([self.p_dot,self.q_dot,self.r_dot]).reshape((3,))
        return np.hstack((r_x_long,r_x_la,pqr_dot,u.reshape((3,))))
        
    def getsys(self):
        return self.plane_sys

if __name__ == '__main__':
    
    sys=Plane()
    print(sys.getsys())